#include <mc_path_planning/path_planning_base.h>


/***************************************
 * 五点梯度下降法
 ***************************************/
// 梯度下降法平滑路径
// https://blog.csdn.net/weixin_43879302/article/details/116092810
// 梯度下降法实现路径平滑
// https://zhuanlan.zhihu.com/p/118666410
void PathPlanningBase::PathSmootherFivePoint(vector<Vec2f> src_points, vector<Vec2f>& out_points){
    if(src_points.size() < 5){
        out_points = src_points;
        return;
    }
    int N=5;
    vector<Vec2f> smooth_points;
    // P = -1/6 * (P-2 - 4P-1 + 6P - 4P1 + P2)
    float a = -1.0 / 6.0;
    for(int n=0; n<N; n++){     //优化N次
        smooth_points.clear();
        smooth_points.resize(src_points.size());
        for(int i=0; i<src_points.size(); ++i){
            Vec2f curr_point = src_points[i];  
            smooth_points[i] = curr_point;
            if(i < 2 || i > src_points.size()-3){
                continue;
            }
            else{
                Vec2f prev_2_point = smooth_points[i-2];
                Vec2f prev_1_point = smooth_points[i-1];
                Vec2f back_1_point = src_points[i+1];
                Vec2f back_2_point = src_points[i+2];
                Vec2f smooth_point = curr_point + a * (prev_2_point 
                    - 4.0 * prev_1_point 
                    + 6.0 * curr_point 
                    - 4.0 * back_1_point
                    + back_2_point);
                // cout << "i: " << i << " " << curr_point << " " << smooth_point << endl;
                Vec2i pixel_smooth = WorldToOpencvPixel(smooth_point);
                if(ValueIsValid(pixel_smooth)){
                    smooth_points[i] = smooth_point;
                }
            }
        }

        // 重新赋值
        src_points = smooth_points;
    }

    out_points.clear();
    out_points.swap(smooth_points);
}


/***************************************
 * 三阶贝塞尔曲线平滑路径
 ***************************************/
// 三阶贝塞尔曲线计算 此处暂不使用递归计算n阶
Vec2f PathPlanningBase::BezierCurve3(const Vec2f& P0, const Vec2f& P1,
                         const Vec2f& P2, const Vec2f& P3, float t){
    float u = 1.0f - t;
    float tt = t * t;
    float uu = u * u;

    // 计算贝塞尔曲线上的点
    Vec2f point;
    point.x = uu * u * P0.x + 3 * uu * t * P1.x + 3 * u * tt * P2.x + tt * t * P3.x;
    point.y = uu * u * P0.y + 3 * uu * t * P1.y + 3 * u * tt * P2.y + tt * t * P3.y;

    return point;
}

// 三阶贝塞尔曲线平滑路径
void PathPlanningBase::PathSmootherBezierCurve3(vector<Vec2f> src_points, vector<Vec2f>& out_points){
    if(src_points.size() < 5){
        out_points = src_points;
        return;
    }
    int N=1;   
    vector<Vec2f> smooth_points;
    for(int n=0; n<N; n++){     //优化N次
        smooth_points.clear();
        smooth_points.resize(src_points.size());
        for(int i=0; i<src_points.size(); ++i){
            Vec2f curr_point = src_points[i];  
            smooth_points[i] = curr_point;
            if(i < 2 || i > src_points.size()-3){
                continue;
            }
            else{
                Vec2f prev_2_point = smooth_points[i-2];
                Vec2f prev_1_point = smooth_points[i-1];
                Vec2f back_1_point = src_points[i+1];
                Vec2f back_2_point = src_points[i+2];
                Vec2f smooth_point = BezierCurve3(
                    prev_2_point, prev_1_point,
                    back_1_point, back_2_point,
                    0.5
                );
                // cout << "i: " << i << " " << curr_point << " " << smooth_point << endl;
                Vec2i pixel_smooth = WorldToOpencvPixel(smooth_point);
                if(ValueIsValid(pixel_smooth)){
                    smooth_points[i] = smooth_point;
                }
            }
        }

        // 重新赋值
        src_points = smooth_points;
    }
    out_points.clear();
    out_points.swap(smooth_points);
}